
#include "register.h"
#include "filter.h"



double getMAE(
    pcl::KdTreeFLANN<PT_TYPE> &kdtree,
    double max_dist, pcl::PointCloud<PT_TYPE>::Ptr cloudtrans)
{
    std::vector<int> nn_indices(1);
    std::vector<float> nn_dists(1);
    double dis_sum = 0, nr = 0;
    for (size_t i = 0; i < cloudtrans->points.size(); ++i)
    {
        // Find its nearest neighbor in the target
        kdtree.nearestKSearch(cloudtrans->points[i], 1, nn_indices, nn_dists);
        // Deal with occlusions (incomplete targets)
        if (nn_dists[0] <= max_dist * max_dist)
        {
            // Add to the fitness score
            dis_sum += std::sqrt(nn_dists[0]);
            nr++;
        }
    }
    return dis_sum / nr;
}

//根据设置的阈值来计算正确匹配的点的比例
void   ComputeRightRatio(PointCloudXYZ &src, PointCloudXYZ &dst, 
                  Eigen::Matrix4d &mt,
                  double  max_distance,
                  double  &min_ratio, 
                  double  &max_ratio)
{
    PointCloudXYZ trans_src = src;
    pcl::transformPointCloud(trans_src, trans_src, mt);

    pcl::KdTreeFLANN<PT_XYZ> kdtree;
    kdtree.setInputCloud( dst.makeShared() );

    std::vector<int> nn_indices(1);
    std::vector<float> nn_dists(1);
    double dis_sum = 0, nr = 0;
    for (size_t i = 0; i < trans_src.size(); ++i)
    {
        // Find its nearest neighbor in the target
        kdtree.nearestKSearch(trans_src[i], 1, nn_indices, nn_dists);
        // Deal with occlusions (incomplete targets)
        if (nn_dists[0] <= max_distance )
        {
            // Add to the fitness score
            dis_sum += std::sqrt(nn_dists[0]);
            nr++;
        }
    }

    max_ratio = max( nr/dst.size(), nr/src.size() );
    min_ratio = min( nr/dst.size(), nr/src.size() );     
}


void addNormal(pcl::PointCloud<PT_TYPE> &cloud,
               pcl::PointCloud<pcl::PointXYZINormal> &cloud_with_normals)
{
    pcl::PointCloud<pcl::Normal> normals;

    //pcl::search::KdTree<PT_TYPE> searchTree;
    pcl::search::KdTree<PT_TYPE>::Ptr searchTree(new pcl::search::KdTree<PT_TYPE>);
    searchTree->setInputCloud(cloud.makeShared());

    pcl::NormalEstimation<PT_TYPE, pcl::Normal> normalEstimator;
    normalEstimator.setInputCloud(cloud.makeShared());
    normalEstimator.setSearchMethod(searchTree);
    normalEstimator.setKSearch(15);
    normalEstimator.compute(normals);

    pcl::concatenateFields(cloud, normals, cloud_with_normals);
}

